#include "gtest/gtest.h"
#include "../BaseUtil/MyTest.h"
#include "MyVideoTrace.h"
namespace MyVideoTrace {

class ModelReconstructorTest: public testing::Test {
public:
	ModelReconstructorTest()
		: point_10_10(Point2D(10, 10)), point_20_30(Point2D(20, 30))
		, point_20_40(Point2D(20,40)), point_10_40(Point2D(10,40)) {}

	class FrameNumber_Point2Ds {
	public:
		typedef vector< Point2D > Points;
		typedef Points::const_iterator PointsIterator;
		typedef Points::size_type PointsIndex;

		FrameNumber_Point2Ds( FrameNumber frameNumber, Point2D point1 ) {
			frameNumber_ = frameNumber;
			addPoint(point1);
		}

		FrameNumber_Point2Ds( FrameNumber frameNumber, Point2D point1, Point2D point2 ) {
			frameNumber_ = frameNumber;
			addPoint(point1);
			addPoint(point2);
		}

		FrameNumber getFrameNumber() const {
			return frameNumber_;
		}

		Point2D getPoint2D(PointsIndex idx) const {
			return points_.at(idx);
		}

		PointsIndex getNumberOfPoint2D() const {
			return points_.size();
		}

		class Comparable {
		public:
			bool operator()(const FrameNumber_Point2Ds & lhs, const FrameNumber_Point2Ds & rhs) {
				if( lhs.getFrameNumber() < rhs.getFrameNumber()) return true;
				if( lhs.getNumberOfPoint2D() < rhs.getNumberOfPoint2D() ) return true;

				Points lhsPoints = lhs.points_;
				Points rhsPoints = rhs.points_;
				sort(lhsPoints.begin(), lhsPoints.end(), comparePoints);
				sort(rhsPoints.begin(), rhsPoints.end(), comparePoints);
				for(PointsIndex idx = 0; idx < lhsPoints.size(); ++idx)
					if( comparePoints( lhsPoints.at(idx), rhsPoints.at(idx) ) ) return true;
				return false;
			}
		};

		bool operator==(const FrameNumber_Point2Ds & rhs) const {
			Comparable comp;
			return false == comp(*this, rhs) && false == comp(rhs, *this);
		}

		friend ostream &operator<<(ostream &stream, FrameNumber_Point2Ds rhs) {
			stream << "FrameNumber_Point2Ds::[ frame = " << rhs.getFrameNumber()
				<< ", size = " << rhs.points_.size()
				<< ", points = [ ";
			for(PointsIterator iter = rhs.points_.begin(); iter != rhs.points_.end(); ++iter)
				stream << (iter == rhs.points_.begin() ? "" : ", ") << *iter;
			return stream << " ] ]";
		}

	private:
		FrameNumber frameNumber_;
		Points points_;

		void addPoint(Point2D aPoint) {
			points_.push_back(aPoint);
		}

		static bool comparePoints(Point2D lhs, Point2D rhs) {
			return lhs.getX() < rhs.getX() || lhs.getY() < rhs.getY();
		}
	};

	typedef set< FrameNumber_Point2Ds, FrameNumber_Point2Ds::Comparable > Inputs;
	typedef Inputs::const_iterator InputsIterator;

	typedef ModelReconstructor::FeaturePoints FeaturePoints;
	typedef FeaturePoints::const_iterator FeaturePointsIterator;
	typedef vector< FeaturePointsIterator > FeaturePointsOrdered;

	typedef ModelReconstructor::FeatureLines FeatureLines;
	typedef FeatureLines::const_iterator FeatureLinesIterator;
	typedef vector< FeatureLinesIterator > FeatureLinesOrdered;

	typedef FeaturePoint::Points2DIterator Points2DIterator;


protected:
	virtual void SetUp() {
		model = new ModelReconstructor();
	    vd = new VoodooLoader();
	}

	virtual void TearDown() {
		delete vd;
	}

	ModelReconstructorPtr model;
	VoodooLoader * vd;

	const Point2D point_10_10;
	const Point2D point_20_30;
	const Point2D point_20_40;
	const Point2D point_10_40;

	FeaturePoints addPoints(Inputs inputs) {
		FeaturePoints outputs;
		for(InputsIterator iter = inputs.begin(); iter != inputs.end(); ++iter)
			for(FrameNumber_Point2Ds::PointsIndex idx = 0; idx < iter->getNumberOfPoint2D(); ++idx)
				outputs.insert( model->addPoint2D( iter->getFrameNumber(), iter->getPoint2D(idx) ) );
		return outputs;
	}

//	FeatureLines addLines(Inputs inputs) {
//		FeatureLines outputs;
//		for(InputsIterator iter = inputs.begin(); iter != inputs.end(); ++iter)
//			outputs.insert( model->addLine2D( iter->getFrameNumber(), iter->getPoint2D(0), iter->getPoint2D(1) ) );
//		return outputs;
//	}

	Inputs revertFeaturePoints(FeaturePoints fpoints) {
		Inputs outputs;
		for(FeaturePointsIterator iter = fpoints.begin(); iter != fpoints.end(); ++iter) {
			for(Points2DIterator iterPoint = (*iter)->beginPoint2D(); iterPoint != (*iter)->endPoint2D(); ++iterPoint)
				outputs.insert( FrameNumber_Point2Ds(iterPoint->first, iterPoint->second) );
		}
		return outputs;
	}

//	FeaturePoints findFeaturePoints(Inputs inputs) {
//		FeaturePoints findResult;
//		for(InputsIterator iter = inputs.begin(); iter != inputs.end(); ++iter)
//			for(FrameNumber_Point2Ds::PointsIndex idx = 0; idx < iter->getNumberOfPoint2D(); ++idx) {
//				const FeaturePointsIterator iterPoint
//					= model->findFeaturePoint( iter->getFrameNumber(), iter->getPoint2D(idx) );
//				findResult.insert( (iterPoint == model->endFeaturePoints()) ? NULL : *iterPoint );
//			}
//		return findResult;
//	}

//	FeatureLines findFeatureLines(Inputs inputs ) {
//		FeatureLines findResults;
//		for(InputsIterator iter = inputs.begin(); iter != inputs.end(); ++iter) {
//			const FeatureLinesIterator iter1 = model->findFeatureLine( iter->getFrameNumber(), iter->getPoint2D(0), iter->getPoint2D(1) );
//			const FeatureLinesIterator iter2 = model->findFeatureLine( iter->getFrameNumber(), iter->getPoint2D(1), iter->getPoint2D(0) );
//			if( iter1 != iter2 ) continue;
//			findResults.insert(*iter1);
//		}
//		return findResults;
//	}

	template< typename ReturnType, typename IterType >
	static ReturnType changeContainer(const IterType & begin, const IterType & end) {
		ReturnType outputs;
		for(IterType iter = begin; iter != end; ++iter)
			outputs.insert( *iter );
		return outputs;
	}

	template< typename Container >
	static bool isThereNull( Container c ) {
		return c.end() != find(c.begin(), c.end(), (void*) NULL);
	}
};
}

namespace MyVideoTrace {
TEST_F(ModelReconstructorTest, InputPointUnorderedSet) {
	Inputs points1;
	points1.insert( FrameNumber_Point2Ds( 0, point_10_10 ) );
	points1.insert( FrameNumber_Point2Ds( 0, point_20_30 ) );
	points1.insert( FrameNumber_Point2Ds( 3, point_20_40 ) );
	points1.insert( FrameNumber_Point2Ds( 0, point_10_10 ) );
	ASSERT_EQ( (size_t) 3, points1.size() );

	Inputs points2;
	points2.insert( FrameNumber_Point2Ds( 3, point_20_40 ) );
	points2.insert( FrameNumber_Point2Ds( 0, point_20_30 ) );
	points2.insert( FrameNumber_Point2Ds( 0, point_10_10 ) );
	ASSERT_EQ( points1, points2 );

	points2.insert( FrameNumber_Point2Ds( 3, point_10_10 ) );
	ASSERT_NE( points1, points2 );
}

TEST_F(ModelReconstructorTest, InputLineUnorderedSet) {
	Inputs lines1;
	lines1.insert( FrameNumber_Point2Ds( 0, point_10_10, point_20_30 ) );
	lines1.insert( FrameNumber_Point2Ds( 0, point_20_30, point_20_40 ) );
	lines1.insert( FrameNumber_Point2Ds( 0, point_20_40, point_10_10 ) );
	lines1.insert( FrameNumber_Point2Ds( 0, point_10_10, point_20_40 ) );
	ASSERT_EQ( (size_t) 3, lines1.size() );

	Inputs lines2;
	lines2.insert( FrameNumber_Point2Ds( 0, point_20_40, point_10_10 ) );
	lines2.insert( FrameNumber_Point2Ds( 0, point_20_30, point_20_40 ) );
	lines2.insert( FrameNumber_Point2Ds( 0, point_10_10, point_20_30 ) );
	ASSERT_EQ( lines1, lines2 );

	lines2.insert( FrameNumber_Point2Ds( 3, point_10_10, point_20_30 ) );
	ASSERT_NE( lines1, lines2 );
}


TEST_F(ModelReconstructorTest, StreamOut) {
	const FrameNumber_Point2Ds input( 0, point_10_10 );

	ostringstream out1;
	out1 << input;
	ASSERT_STREQ("FrameNumber_Point2Ds::[ frame = 0, size = 1, points = [ Point2D::[ x = 10, y = 10 ] ] ]", out1.str().c_str());

	Inputs inputs;
	inputs.insert( FrameNumber_Point2Ds( 3, point_20_40 ) );

	std::ostringstream out2;
	MyTest::operator<<(out2, inputs);
	ASSERT_STREQ("set::[ size = 1, values = FrameNumber_Point2Ds::[ frame = 3, size = 1, points = [ Point2D::[ x = 20, y = 40 ] ] ] ]",
			out2.str().c_str());
}

// ----------------- Matrix ------------------------
TEST_F(ModelReconstructorTest, CameraMatrix1) {
	VoodooLoader vd;
	vd.loadCameraData("../VoodooData/car.txt");
	vd.loadFeaturePoints( "../VoodooData/car", 0, 10, ".pnt", 1);

	VoodooCameraInfo camera = vd.getCameraInfo(3);
	FeaturePoint fpoint = vd.getFeaturePoint(1);
	Vector3f posIn3D = fpoint.getPoint3D();
	Vector3f rst = ModelReconstructor::getProjectionPosition( camera, posIn3D );
	ASSERT_NEAR( fpoint.findPoint2D(3)->second.getX(), rst.getX() / rst.getZ() + camera.getImageSizeWidth() / 2 -1, 2 ) << "Feature Point ID: " << fpoint.getFeaturePointID();
	ASSERT_NEAR( fpoint.findPoint2D(3)->second.getY(), rst.getY() / rst.getZ() + camera.getImageSizeHeight() / 2 -1, 2 ) << "Feature Point ID: " << fpoint.getFeaturePointID();
}

TEST_F(ModelReconstructorTest, Projection) {
	VoodooLoader vd;
	vd.loadCameraData("../VoodooData/car.txt");
	vd.loadFeaturePoints( "../VoodooData/car", 0, 10, ".pnt", 1);
	VoodooCameraInfo camera = vd.getCameraInfo(3);
	FeaturePoint fpoint = vd.getFeaturePoint(1);
	Vector3f proj = ModelReconstructor::getProjectionPosition(camera, fpoint.getPoint3D() );
	ASSERT_NEAR( fpoint.findPoint2D(3)->second.getX(), proj.getX() / proj.getZ() + camera.getImageSizeWidth() / 2 -1, 2 );
	ASSERT_NEAR( fpoint.findPoint2D(3)->second.getY(), proj.getY() / proj.getZ() + camera.getImageSizeHeight() / 2 -1, 2 );
}

//-----------------FeaturePoints--------------------

TEST_F(ModelReconstructorTest, AddPointsAndObserve) {

//	ASSERT_FALSE( model->hasFeaturePoint(0, point_10_10) );
//	ASSERT_EQ( (size_t) 0, model->sizeFeaturePoints() );
//	model->addPoint2D( 0, point_10_10 );
//	ASSERT_EQ( (size_t) 1, model->sizeFeaturePoints() );
//	ASSERT_TRUE( model->hasFeaturePoint(0, point_10_10) );
}

TEST_F(ModelReconstructorTest, AddPointsAndCheckFeatruePoints) {

//	Inputs inputs;
//	inputs.insert( FrameNumber_Point2Ds( 0, point_10_10 ) );
//	inputs.insert( FrameNumber_Point2Ds( 0, point_20_30 ) );
//	inputs.insert( FrameNumber_Point2Ds( 3, point_20_40 ) );
//
//	const FeaturePoints fpoints = addPoints(inputs);
//	ASSERT_EQ( inputs.size(), fpoints.size() );
//	ASSERT_EQ( model->sizeFeaturePoints(), fpoints.size() );
//	ASSERT_FALSE( isThereNull(fpoints) );
//
//	const Inputs outputs = revertFeaturePoints(fpoints);
//	ASSERT_EQ( fpoints.size(), outputs.size() );
//	ASSERT_EQ( model->sizeFeaturePoints(), outputs.size() );
//
//	ASSERT_EQ( inputs, outputs ) << "Generated feature point doesn't match with the original points";
}

TEST_F(ModelReconstructorTest, AddTheSamePointAtDifferentFrame) {

//	Inputs inputs;
//	inputs.insert( FrameNumber_Point2Ds( 0, point_10_10 ) );
//	inputs.insert( FrameNumber_Point2Ds( 3, point_10_10 ) );
//
//	const FeaturePoints fpoints = addPoints(inputs);
//	ASSERT_EQ( inputs.size(), fpoints.size()) << "The same point in a different frame is not addable.";
//	ASSERT_EQ( model->sizeFeaturePoints(), fpoints.size() );
//	ASSERT_FALSE( isThereNull(fpoints) );
//
//	const Inputs outputs = revertFeaturePoints(fpoints);
//	ASSERT_EQ( fpoints.size(), outputs.size() );
//	ASSERT_EQ( model->sizeFeaturePoints(), outputs.size() );
//
//	ASSERT_EQ( inputs, outputs ) << "Generated feature point doesn't match with the original points";
}

TEST_F(ModelReconstructorTest, AddTheSamePointAtTheSameFrame) {

//	Inputs inputs;
//	inputs.insert( FrameNumber_Point2Ds( 0, point_10_10 ) );
//
//	const FeaturePoints fpointsNormal = addPoints(inputs);
//	ASSERT_EQ( inputs.size(), fpointsNormal.size());
//	ASSERT_EQ( model->sizeFeaturePoints(), fpointsNormal.size());
//	ASSERT_FALSE( isThereNull(fpointsNormal) );
//
//	const FeaturePoints fpointsEmpty = addPoints(inputs);
//	ASSERT_TRUE( isThereNull(fpointsEmpty) ) << "Redundant points are addable.";
}

TEST_F(ModelReconstructorTest, CheckIteratorForFeaturePoints) {
//	Inputs inputs;
//	inputs.insert( FrameNumber_Point2Ds( 0, point_10_10 ) );
//	inputs.insert( FrameNumber_Point2Ds( 0, point_20_30 ) );
//	inputs.insert( FrameNumber_Point2Ds( 3, point_20_40 ) );
//
//	const FeaturePoints expected = addPoints(inputs);
//	ASSERT_EQ( inputs.size(), expected.size() );
//	ASSERT_EQ( model->sizeFeaturePoints(), expected.size());
//	ASSERT_FALSE( isThereNull(expected) );


//	const FeaturePoints outputs = changeContainer<FeaturePoints>(model->beginFeaturePoints(), model->endFeaturePoints());
//	ASSERT_EQ( expected, outputs );
}

//-----------------Feature Lines--------------------

TEST_F(ModelReconstructorTest, AddLinesAndObserve) {
//	model->addPoint2D( 0, point_10_10 );
//	model->addPoint2D( 0, point_20_30 );


//	ASSERT_EQ( (size_t) 0, model->sizeFeatureLines() );
//	model->addLine2D( 0, point_10_10, point_20_30 );
//	ASSERT_EQ( (size_t) 1, model->sizeFeatureLines() );
//	ASSERT_TRUE( model->hasFeatureLine(0, point_20_30, point_10_10) );
}

TEST_F(ModelReconstructorTest, AddLinesAndCheckFeatureLines) {
//	Inputs inputs;
//	inputs.insert( FrameNumber_Point2Ds( 0, point_10_10, point_20_30 ) );
//	inputs.insert( FrameNumber_Point2Ds( 0, point_20_30, point_20_40 ) );
//	inputs.insert( FrameNumber_Point2Ds( 0, point_20_40, point_10_10 ) );
//
//	addPoints(inputs);


//	const FeatureLines expected = addLines(inputs);
//	ASSERT_EQ( inputs.size(), expected.size() );
//	ASSERT_EQ( model->sizeFeatureLines(), expected.size());
//	ASSERT_FALSE( isThereNull(expected) );
//
//	const FeatureLines outputs = changeContainer<FeatureLines>(model->beginFeatureLines(), model->endFeatureLines());
//	ASSERT_EQ( model->sizeFeatureLines(), outputs.size());
//
//	ASSERT_EQ( expected, outputs ) << "Generated feature point doesn't match with the original points";
}

TEST_F(ModelReconstructorTest, AddTheSameLineAtDifferentFrame) {
//	Inputs inputs;
//	inputs.insert( FrameNumber_Point2Ds( 0, point_10_10, point_20_30 ) );
//	inputs.insert( FrameNumber_Point2Ds( 1, point_10_10, point_20_30 ) );
//
//	addPoints(inputs);


//	const FeatureLines expected = addLines(inputs);
//	ASSERT_EQ( inputs.size(), expected.size() );
//	ASSERT_EQ( model->sizeFeatureLines(), expected.size());
//	ASSERT_FALSE( isThereNull(expected) );
//
//	const FeatureLines outputs = changeContainer<FeatureLines>(model->beginFeatureLines(), model->endFeatureLines());
//	ASSERT_EQ( model->sizeFeatureLines(), outputs.size());
//
//	ASSERT_EQ( expected, outputs ) << "Generated feature point doesn't match with the original points";
}

TEST_F(ModelReconstructorTest, AddTheSameLineAtTheSameFrame) {
//	Inputs inputs;
//	inputs.insert( FrameNumber_Point2Ds( 0, point_10_10, point_20_30 ) );
//
//	addPoints(inputs);


//	const FeatureLines linesNormal = addLines(inputs);
//	ASSERT_EQ( inputs.size(), linesNormal.size() );
//	ASSERT_EQ( model->sizeFeatureLines(), linesNormal.size());
//	ASSERT_FALSE( isThereNull(linesNormal) );
//
//	const FeatureLines linesEmpty = addLines(inputs);
//	ASSERT_TRUE( isThereNull(linesEmpty) ) << linesEmpty;
//
//	ASSERT_EQ( inputs.size(), model->sizeFeatureLines()) << "Redundant lines are added.";
}

TEST_F(ModelReconstructorTest, AddTheSameLineAtTheSameFrameInOppositeDirection) {
//	const Point2D point1 = point_10_10;
//	const Point2D point2 = point_20_40;
//
//	model->addPoint2D( 0, point1 );
//	model->addPoint2D( 0, point2 );


//	const FeatureLinePtr lineNormal = model->addLine2D( 0, point1, point2 );
//	ASSERT_EQ((size_t) 1, model->sizeFeatureLines());
//
//	const FeatureLinePtr lineEmpty = model->addLine2D( 0, point2, point1 );
//	ASSERT_FALSE( lineEmpty ) << "Must be NULL";
//	ASSERT_EQ((size_t)1, model->sizeFeatureLines());
}

TEST_F(ModelReconstructorTest, ConnectTwoPointsThatAreInDifferentFrames) {
//	const Point2D point1 = point_10_10;
//	const Point2D point2 = point_20_40;
//
//	model->addPoint2D( 0, point1 );
//	model->addPoint2D( 1, point2 );


//	const FeatureLinePtr lineEmpty = model->addLine2D( 0, point1, point2 );
//	ASSERT_FALSE( lineEmpty ) << "Must be NULL";
//	ASSERT_EQ( (size_t) 0, model->sizeFeatureLines());
}

// ----------- find points -----------------------------------

TEST_F(ModelReconstructorTest, FindFeaturePoints) {
//	Inputs inputs;
//	inputs.insert( FrameNumber_Point2Ds( 0, point_10_10 ) );
//	inputs.insert( FrameNumber_Point2Ds( 0, point_20_30 ) );
//	inputs.insert( FrameNumber_Point2Ds( 3, point_20_30 ) );


//	const FeaturePoints expected = addPoints(inputs);
//	const FeaturePoints findResult = findFeaturePoints(inputs);
//	ASSERT_EQ( expected, findResult );
}

TEST_F(ModelReconstructorTest, NearFindFeaturePoints) {
//	const Point2D point1 = point_10_10;
//	const Point2D point2 = point_20_30;
//	const Point2D point3 = point_20_30;
//
//	const FeaturePointPtr fpoint1 = model->addPoint2D( 0, point1 );
//	const FeaturePointPtr fpoint2 = model->addPoint2D( 0, point2 );
//	const FeaturePointPtr fpoint3 = model->addPoint2D( 3, point3 );


//	FeaturePointsOrdered founds;
//	founds.push_back(model->findNearFeaturePoint( 0, point1 + Point2D(4, 4), 6.f ));
//	founds.push_back(model->findNearFeaturePoint( 0, point2 - Point2D(4, 3), 5.f ));
//	founds.push_back(model->findNearFeaturePoint( 3, point3, 0.f) );
//
//	ASSERT_TRUE( model->endFeaturePoints() != founds[0] );
//	ASSERT_TRUE( model->endFeaturePoints() != founds[1] );
//	ASSERT_TRUE( model->endFeaturePoints() != founds[2] );
//	ASSERT_EQ( fpoint1, *founds[0] );
//	ASSERT_EQ( fpoint2, *founds[1] );
//	ASSERT_EQ( fpoint3, *founds[2] );
//
//	ASSERT_TRUE( model->endFeaturePoints() == model->findNearFeaturePoint( 0, point2 - Point2D(4, 3), 4.9f) ) << "Out bounded point is searched.";
}

TEST_F(ModelReconstructorTest, FindFeatureLines) {
//	Inputs inputs;
//	inputs.insert( FrameNumber_Point2Ds( 0, point_10_10, point_20_30) );
//	inputs.insert( FrameNumber_Point2Ds( 0, point_20_30, point_20_40 ) );
//	inputs.insert( FrameNumber_Point2Ds( 0, point_20_40, point_10_10 ) );


//	addPoints(inputs);
//	const FeatureLines expected = addLines(inputs);
//	const FeatureLines findResults = findFeatureLines(inputs);
//	ASSERT_EQ(expected, findResults);
}

TEST_F(ModelReconstructorTest, FindFeatureLinesInWrongFrame) {
//	const Point2D point1 = point_10_10;
//	const Point2D point2 = point_20_30;


//	model->addPoint2D(0, point1);
//	model->addPoint2D(0, point2);
//	model->addLine2D(0, point1, point2);
//
//	ASSERT_TRUE( model->endFeatureLines() != model->findFeatureLine( 0, point1, point2 ) );
//	ASSERT_TRUE( model->endFeatureLines() == model->findFeatureLine( 1, point1, point2 ) ) << "A line from a wrong frame is searched.";
}

TEST_F(ModelReconstructorTest, NearFindFeatureLines) {
//	const Point2D point1 = point_10_10;
//	const Point2D point2 = point1 + Point2D(60, 0);
//	const Point2D point3 = point1 + Point2D(0, 60);
//
//	model->addPoint2D(0, point1);
//	model->addPoint2D(0, point2);
//	model->addPoint2D(0, point3);


//	const FeatureLinePtr line1 = model->addLine2D(0, point1, point2);
//	const FeatureLinePtr line2 = model->addLine2D(0, point2, point3);
//	const FeatureLinePtr line3 = model->addLine2D(0, point3, point1);
//	ASSERT_TRUE( line1 && line2 && line3 );
//
//	FeatureLinesOrdered founds;
//	founds.push_back( model->findNearFeatureLine( 0, point1 + (point2 - point1) / 2 + Point2D(0, 5), 5.f ) );
//	founds.push_back( model->findNearFeatureLine( 0, point2 + (point3 - point2) / 2 + Point2D(3, 3), 4.3f ) );
//	founds.push_back( model->findNearFeatureLine( 0, point2 + (point3 - point2) / 7 + Point2D(4, 3), 5.f ) );
//	founds.push_back( model->findNearFeatureLine( 0, point1 + (point3 - point1) / 60, 0.f ) );
//
//	ASSERT_TRUE( model->endFeatureLines() != founds[0] );
//	ASSERT_TRUE( model->endFeatureLines() != founds[1] );
//	ASSERT_TRUE( model->endFeatureLines() != founds[2] );
//	ASSERT_TRUE( model->endFeatureLines() != founds[3] );
//	ASSERT_EQ( line1, *founds[0] );
//	ASSERT_EQ( line2, *founds[1] );
//	ASSERT_EQ( line2, *founds[2] );
//	ASSERT_EQ( line3, *founds[3] );
//
//	ASSERT_TRUE( model->endFeatureLines() == model->findNearFeatureLine( 0, point2 + (point3 - point2) / 2 + Point2D(3, 3), 4.2f ) ) << "Out bounded line is searched.";
}

// ---------- Remove Feature Lines --------------------------

TEST_F(ModelReconstructorTest, RemoveFeatureLines) {
//	const Point2D point1 = point_10_10;
//	const Point2D point2 = point_20_30;
//	const Point2D point3 = point_20_40;
//
//	model->addPoint2D( 0, point1 );
//	model->addPoint2D( 0, point2 );
//	model->addPoint2D( 0, point3 );


//	const FeatureLinePtr line1 = model->addLine2D( 0, point1, point2 );
//	const FeatureLinePtr line2 = model->addLine2D( 0, point2, point3 );
//	const FeatureLinePtr line3 = model->addLine2D( 0, point3, point1 );
//
//	ASSERT_EQ( (size_t) 3, model->sizeFeatureLines());
//	model->removeLine2D(0, point2, point3);
//	ASSERT_EQ( (size_t) 2, model->sizeFeatureLines());
//
//	ASSERT_TRUE( model->hasFeatureLine(0, point1, point2 ) );
//	ASSERT_TRUE( model->hasFeatureLine(0, point3, point1 ) );
}

TEST_F(ModelReconstructorTest, RemoveFeaturePoints) {
//	const Point2D point1 = point_10_10;
//	const Point2D point2 = point_20_30;
//	const Point2D point3 = point_20_40;
//
//	const FeaturePointPtr fpoint1 = model->addPoint2D( 0, point1 );
//	const FeaturePointPtr fpoint2 = model->addPoint2D( 0, point2 );
//	const FeaturePointPtr fpoint3 = model->addPoint2D( 0, point3 );


//	const FeatureLinePtr line1 = model->addLine2D( 0, point1, point2 );
//	const FeatureLinePtr line2 = model->addLine2D( 0, point2, point3 );
//	const FeatureLinePtr line3 = model->addLine2D( 0, point3, point1 );
//
//	model->removePoint2D(0, point2);
//	ASSERT_EQ((size_t)2, model->sizeFeaturePoints());
//	ASSERT_EQ((size_t)1, model->sizeFeatureLines());
//
//	ASSERT_TRUE( model->hasFeaturePoint(0, point1) );
//	ASSERT_TRUE( model->hasFeaturePoint(0, point3) );
//
//	ASSERT_TRUE( model->hasFeatureLine(0, point3, point1) );
}

// ----------------- move point ---------------------------

TEST_F(ModelReconstructorTest, MoveFeaturePoint) {
//	const Point2D point1 = point_10_10;
//	const Point2D point2 = point_20_30;
//
//	model->addPoint2D( 0, point1 );
//
//	ASSERT_EQ((size_t)1, model->sizeFeaturePoints());
//	model->movePoint2D( 0, point1, point2);
//	ASSERT_EQ((size_t)1, model->sizeFeaturePoints());
//
//	ASSERT_TRUE( model->hasFeaturePoint(0, point2) ) << "Point2D is not moved.";
//	ASSERT_FALSE( model->hasFeaturePoint(0, point1) ) << "Point2D is duplicated while moving.";
}

TEST_F(ModelReconstructorTest, MoveFeaturePointToPreventOverlab) {
//	const Point2D point1 = point_10_10;
//	const Point2D point2 = point_20_30;
//
//	model->addPoint2D( 0, point1 );
//	model->addPoint2D( 0, point2 );
//
//	ASSERT_EQ((size_t)2, model->sizeFeaturePoints());
//	model->movePoint2D( 0, point1, point2);
//	ASSERT_EQ((size_t)2, model->sizeFeaturePoints());
//
//	ASSERT_TRUE( model->hasFeaturePoint(0, point1) );
//	ASSERT_TRUE( model->hasFeaturePoint(0, point2) );
}

TEST_F(ModelReconstructorTest, MoveSingleFeatureLine) {
//	const Point2D point1 = point_10_10;
//	const Point2D point2 = point_20_30;
//	const Point2D point3 = point_20_40;
//

//	model->addPoint2D( 0, point1 );
//	model->addPoint2D( 0, point2 );
//	model->addLine2D( 0, point1, point2 );
//
//	ASSERT_EQ((size_t)1, model->sizeFeatureLines());
//	ASSERT_EQ((size_t)2, model->sizeFeaturePoints());
//	model->movePoint2D( 0, point1, point3);
//	ASSERT_EQ((size_t)1, model->sizeFeatureLines());
//	ASSERT_EQ((size_t)2, model->sizeFeaturePoints());
//
//	ASSERT_FALSE( model->hasFeaturePoint(0, point1) );
//	ASSERT_TRUE( model->hasFeaturePoint(0, point2) );
//	ASSERT_TRUE( model->hasFeaturePoint(0, point3) );
//
//	ASSERT_FALSE( model->hasFeatureLine(0, point1, point2) );
//	ASSERT_TRUE( model->hasFeatureLine(0, point3, point2) );
}

TEST_F(ModelReconstructorTest, MoveMultiFeatureLine) {
	const Point2D point1 = point_10_10;
	const Point2D point2 = point_20_30;
	const Point2D point3 = point_20_40;
	const Point2D point4 = point_10_40;


//	model->addPoint2D( 0, point1 );
//	model->addPoint2D( 0, point2 );
//	model->addPoint2D( 0, point3 );
//	model->addLine2D( 0, point1, point2 );
//	model->addLine2D( 0, point1, point3 );
//
//	ASSERT_EQ((size_t)2, model->sizeFeatureLines());
//	ASSERT_EQ((size_t)3, model->sizeFeaturePoints());
//	model->movePoint2D( 0, point1, point4);
//	ASSERT_EQ((size_t)2, model->sizeFeatureLines());
//	ASSERT_EQ((size_t)3, model->sizeFeaturePoints());
//
//	ASSERT_FALSE( model->hasFeaturePoint(0, point1) );
//	ASSERT_TRUE( model->hasFeaturePoint(0, point2) );
//	ASSERT_TRUE( model->hasFeaturePoint(0, point3) );
//	ASSERT_TRUE( model->hasFeaturePoint(0, point4) );
//
//	ASSERT_FALSE( model->hasFeatureLine(0, point1, point2) );
//	ASSERT_FALSE( model->hasFeatureLine(0, point1, point3) );
//	ASSERT_TRUE( model->hasFeatureLine(0, point4, point2) );
//	ASSERT_TRUE( model->hasFeatureLine(0, point4, point3) );
}

TEST_F(ModelReconstructorTest, ImportVoodooData) {
    ASSERT_TRUE( vd->loadCameraData("../VoodooData/car.txt") );
	ASSERT_TRUE( vd->loadFeaturePoints( "../VoodooData/car", 0, 10, ".pnt", 1) );

	model->importVoodoo( vd );
	ASSERT_EQ( vd, model->voodooLoader_ );
}

TEST_F(ModelReconstructorTest, ProjectionOfFeaturePoint) {
//	VoodooCameraInfo camera = voodooLoader_->getCameraInfo( frameNumber );
//	CvMat M1 = getCameraMatrix1( camera );
//	CvMat M2 = getCameraMatrix2( camera );
//	CvMat Mtmp1 = getTemporary3by1Matrix();
//	CvMat Mtmp2 = getTemporary3by1Matrix();
}


}
